#pragma warning(disable:4244)

#include "gmTorBezObj.h"

#include "gmEvaluatorStatic.h"
#include <sstream>

TorBez::TorBez( DVector<Vector<double,3>> pts ){
	_pts.setDim( pts.getDim() );
	_bernsteinPoly.setDim( pts.getDim()+1, pts.getDim()+1 );


	for( int i = 0; i < pts.getDim(); i++ )
	{
		_pts[i][0] = pts[i][0];
		_pts[i][1] = pts[i][1];
		_pts[i][2] = pts[i][2];
	}
	float siz = 10;
	setSurroundingSphere( Sphere<GLfloat,3>( Point3D<GLfloat>(0.0,0.0,0.0), siz ) );
}

 void TorBez::localDisplay() {
	glColor3f( 0.0, 0.0, 1.0 );
  glDisable(GL_LIGHTING);
	glPointSize(2.0);
	glEnable(GL_POINT_SMOOTH);
  glBegin(GL_POINTS);
	for(int i=0; i< _pts.getDim(); i++)
	{
		glVertex3f( _pts[i][0], _pts[i][1], _pts[i][2] );
	}
	glEnd();

	glColor3f( 0.0, 1.0, 0.0 );
	glBegin(GL_LINE_STRIP);
	int steps = 30;
	for(int i=0; i<steps; i++)
	{
		double t = (double) i * 1.0 / (steps-1);
		Vector<double,3> p = eval(t,1);
		glVertex3f( p[0], p[1], p[2] );
	}
   
  glEnd();
  glEnable(GL_LIGHTING);



  // Display the surrounding sphere
  // _sphere->display();
 }


 void TorBez::localSelect() {

	 glBegin(GL_POINTS);
	 for(int i=0; i< _pts.getDim(); i++)
	 {
		 glVertex3f( _pts[i][0], _pts[i][1], _pts[i][2] );
	 }

	 glEnd();
 }


 void TorBez::localSimulate( double dt ) {

  rotate( Angle(50) * dt, Vector3D<GLfloat>( 1.0, 1.0, 0.0 ) );
 }


 void TorBez::bernstein(int d,double t,double delta)
 {
	 _bernsteinPoly.setDim(d+1,d+1);

	 _bernsteinPoly[d-1][0] = 1-t;
	 _bernsteinPoly[d-1][1] = t;
	 for(int i=d-2; i>=0; i--)
	 {
		 _bernsteinPoly[i][0] = (1-t) * _bernsteinPoly[i+1][0];
		 for(int j=1; j<d-i; j++)
		 {
			 _bernsteinPoly[i][j] = t*_bernsteinPoly[i+1][j-1] + (1-t)*_bernsteinPoly[i+1][j];
		 }
		 _bernsteinPoly[i][d-i] = t*_bernsteinPoly[i+1][d-i-1];

	 }

	 _bernsteinPoly[d][0] = -delta;
	 _bernsteinPoly[d][1] = delta;
	 for(int k=2; k<=d; k++)
	 {
		 double s = k*delta;
		 for(int i=d; i>d-k; i--)
		 {
			 _bernsteinPoly[i][k] = s*_bernsteinPoly[i][k-1];
			 for(int j=k-1; j>0; j--)
			 {
				 _bernsteinPoly[i][j] = s*(_bernsteinPoly[i][j-1]-_bernsteinPoly[i][j]);
			 }
			 _bernsteinPoly[i][0] = -s*_bernsteinPoly[i][0];
		 }
	 }

 }


Vector<double,3> TorBez::eval( double t, int d ) {
	bernstein(_pts.getDim()-1,t,1.0);

	DVector< Vector<double,3> > pt = _bernsteinPoly * _pts; // Multiply basis functions with points
	this->_p[0] = (double) pt[0][0]; // point to be plotted
	this->_p[1] = (double) pt[0][1];
	this->_p[2] = (double) pt[0][2];

	return _p;
	//this->translate( Vector3D<double>( _pts[0][0], _pts[0][1], _pts[0][2]) );
}